%figure 4 the other direction
%figure3
clear;

cnt = 0;
mpopt = mpoption('out.all', 0, 'verbose', 0);
mpc = ext2int(case39);

mpc.baseMVA = 345;


gen_buses = find(mpc.bus(:, 2) == 2 | mpc.bus(:, 2) == 3);
load_buses = find(mpc.bus(:, 2) == 1);

the = [];
num = [];
Delta = [];
theo = [];

mpc.bus(:,4) = (1/0.88^2-1)*mpc.bus(:,3);

%lambda
lambda = 0: 0.01: 5;
for i = 1:length(lambda)
    mpc_lam = mpc;
    %active and reactive power demands and generation
    mpc_lam.bus(:, 3) = (1 + lambda(i))  * mpc_lam.bus(:, 3);
    mpc_lam.bus(:, 4) = (1 + lambda(i))  * mpc_lam.bus(:, 4);
    %computed voltage magnitude at node four
    [result, success] = runpf(mpc_lam, mpopt);
    if success == 1
        cnt = cnt + 1;
        num(i) = result.bus(4,8);
        %theoretical bound
        Ybus = full(makeYbus(mpc_lam));
        Ybus = imag(Ybus);
        BGG = Ybus(gen_buses, gen_buses);
        BLL = Ybus(load_buses, load_buses);
        BLG = Ybus(load_buses, gen_buses);
        [~, genidx] = ismember(gen_buses, mpc_lam.gen(:,1));
        VG =  mpc_lam.gen(genidx,6);
        V_L_star = -inv(BLL) * BLG * VG;
        Q_crit = 0.25 * diag(V_L_star) * BLL * diag(V_L_star);
        QL = mpc_lam.bus(load_buses, 4)/mpc_lam.baseMVA;
        temp = Q_crit\QL;
        Delta(i) = norm(temp, "inf");
        if Delta(i) <= 1
            delta_predict = (1 - sqrt(1 - Delta(i)))/2;
            the(i) = (1 - delta_predict) * V_L_star(4);
        else
            disp("collapse");
            lambda_collapse = lambda(i);
            the(i) = (1 - delta_predict) * V_L_star(4);
            Delta(i)
            break;
        end
    else
        disp("collapse");
        lambda_collapse = lambda(i);
        break;
    end
end
lambda = lambda(1:length(num));

plot(lambda./lambda_collapse, the,'r.', lambda./lambda_collapse, num,'k',lambda./lambda_collapse, Delta,'b--');

axis equal;
axis([0 1 0 1.1]);


